/*******************************************************************************
 #	 	luvcview: Sdl video Usb Video Class grabber          .         #
 #This package work with the Logitech UVC based webcams with the mjpeg feature. #
 #All the decoding is in user space with the embedded jpeg decoder              #
 #.                                                                             #
 # 		Copyright (C) 2005 2006 Laurent Pinchart &&  Michel Xhaard     #
 #                                                                              #
 # This program is free software; you can redistribute it and/or modify         #
 # it under the terms of the GNU General Public License as published by         #
 # the Free Software Foundation; either version 2 of the License, or            #
 # (at your option) any later version.                                          #
 #                                                                              #
 # This program is distributed in the hope that it will be useful,              #
 # but WITHOUT ANY WARRANTY; without even the implied warranty of               #
 # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the                #
 # GNU General Public License for more details.                                 #
 #                                                                              #
 # You should have received a copy of the GNU General Public License            #
 # along with this program; if not, write to the Free Software                  #
 # Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA    #
 #                                                                              #
 *******************************************************************************/



#include "utils.h"
#include "color.h"
#include <stdio.h>
#include <stdlib.h>
#include <unistd.h>
#include <linux/types.h>
#include <string.h>
#include <fcntl.h>
//#include <wait.h>
#include <time.h>
#include <limits.h>
#include "huffman.h"

#ifdef __cplusplus
extern "C" {
#endif

#define ISHIFT 11

#define IFIX(a) ((int)((a) * (1 << ISHIFT) + .5))

#ifndef __P
#define __P(x) x
#endif

/* special markers */
#define M_BADHUFF	-1
#define M_EOF		0x80

struct jpeg_decdata
{
	int dcts[6 * 64 + 16];
	int out[64 * 6];
	int dquant[3][64];
};

struct in
{
	unsigned char *p;
	unsigned int bits;
	int left;
	int marker;
	int (*func)__P((void *));
	void *data;
};

/*********************************/
struct dec_hufftbl;
struct enc_hufftbl;

union hufftblp
{
	struct dec_hufftbl *dhuff;
	struct enc_hufftbl *ehuff;
};

struct scan
{
	int dc; /* old dc value */

	union hufftblp hudc;
	union hufftblp huac;
	int next; /* when to switch to next scan */

	int cid; /* component id */
	int hv; /* horiz/vert, copied from comp */
	int tq; /* quant tbl, copied from comp */
};

/*********************************/

#define DECBITS 10		/* seems to be the optimum */

struct dec_hufftbl
{
	int maxcode[17];
	int valptr[16];
	unsigned char vals[256];
	unsigned int llvals[1 << DECBITS];
};
static int huffman_init(void);
static void decode_mcus
__P((struct in *, int *, int, struct scan *, int *));

static int dec_readmarker
__P((struct in *));

static void dec_makehuff
__P((struct dec_hufftbl *, int *, unsigned char *));

static void setinput
__P((struct in *, unsigned char *));
/*********************************/

#undef PREC
#define PREC int

static void idctqtab
__P((unsigned char *, PREC *));

inline static void idct(int *in, int *out, int *quant, long off, int max);

int is_huffman(unsigned char *buf);

/*********************************/

static void yuv420pto422(int * out, unsigned char *pic, int width);
static void yuv422pto422(int * out, unsigned char *pic, int width);
static void yuv444pto422(int * out, unsigned char *pic, int width);
static void yuv400pto422(int * out, unsigned char *pic, int width);
typedef void (*ftopict)(int *out, unsigned char *pic, int width);
/*********************************/

#define M_SOI	0xd8
#define M_APP0	0xe0
#define M_DQT	0xdb
#define M_SOF0	0xc0
#define M_DHT   0xc4
#define M_DRI	0xdd
#define M_SOS	0xda
#define M_RST0	0xd0
#define M_EOI	0xd9
#define M_COM	0xfe

static unsigned char *datap;

static int getbyte(void)
{
	return *datap++;
}

static int getword(void)
{
	int c1, c2;
	c1 = *datap++;
	c2 = *datap++;
	return c1 << 8 | c2;
}

struct comp
{
	int cid;
	int hv;
	int tq;
};

#define MAXCOMP 4
struct jpginfo
{
	int nc; /* number of components */
	int ns; /* number of scans */
	int dri; /* restart interval */
	int nm; /* mcus til next marker */
	int rm; /* next restart marker */
};

static struct jpginfo info;
static struct comp comps[MAXCOMP];

static struct scan dscans[MAXCOMP];

static unsigned char quant[4][64];

static struct dec_hufftbl dhuff[4];

#define dec_huffdc (dhuff + 0)
#define dec_huffac (dhuff + 2)

static struct in in;

static int readtables(int till, int *isDHT)
{
	int m, l, i, j, lq, pq, tq;
	int tc, th, tt;

	for (;;)
	{
		if (getbyte() != 0xff)
			return -1;
		if ((m = getbyte()) == till)
			break;

		switch (m)
		{
		case 0xc2:
			return 0;

		case M_DQT:
			//printf("find DQT \n");
			lq = getword();
			while (lq > 2)
			{
				pq = getbyte();
				tq = pq & 15;
				if (tq > 3)
					return -1;
				pq >>= 4;
				if (pq != 0)
					return -1;
				for (i = 0; i < 64; i++)
					quant[tq][i] = getbyte();
				lq -= 64 + 1;
			}
			break;

		case M_DHT:
			//printf("find DHT \n");
			l = getword();
			while (l > 2)
			{
				int hufflen[16], k;
				unsigned char huffvals[256];

				tc = getbyte();
				th = tc & 15;
				tc >>= 4;
				tt = tc * 2 + th;
				if (tc > 1 || th > 1)
					return -1;
				for (i = 0; i < 16; i++)
					hufflen[i] = getbyte();
				l -= 1 + 16;
				k = 0;
				for (i = 0; i < 16; i++)
				{
					for (j = 0; j < hufflen[i]; j++)
						huffvals[k++] = getbyte();
					l -= hufflen[i];
				}
				dec_makehuff(dhuff + tt, hufflen, huffvals);
			}
			*isDHT = 1;
			break;

		case M_DRI:
			//printf("find DRI \n");
			l = getword();
			info.dri = getword();
			break;

		default:
			l = getword();
			while (l-- > 2)
				getbyte();
			break;
		}
	}

	return 0;
}

static void dec_initscans(void)
{
	int i;

	info.nm = info.dri + 1;
	info.rm = M_RST0;
	for (i = 0; i < info.ns; i++)
		dscans[i].dc = 0;
}

static int dec_checkmarker(void)
{
	int i;

	if (dec_readmarker(&in) != info.rm)
		return -1;
	info.nm = info.dri;
	info.rm = (info.rm + 1) & ~0x08;
	for (i = 0; i < info.ns; i++)
		dscans[i].dc = 0;
	return 0;
}

int jpeg_decode(unsigned char **pic, unsigned char *buf, int *width,
		int *height)
{
	struct jpeg_decdata *decdata;
	int i, j, m, tac, tdc;
	int intwidth, intheight;
	int mcusx, mcusy, mx, my;
	int ypitch, xpitch, bpp, pitch, x, y;
	int mb;
	int max[6];
	ftopict convert;
	int err = 0;
	int isInitHuffman = 0;
	decdata = (struct jpeg_decdata *) malloc(sizeof(struct jpeg_decdata));

	if (!decdata)
	{
		err = -1;
		goto error;
	}
	if (buf == NULL)
	{
		err = -1;
		goto error;
	}
	datap = buf;
	if (getbyte() != 0xff)
	{
		err = ERR_NO_SOI;
		goto error;
	}
	if (getbyte() != M_SOI)
	{
		err = ERR_NO_SOI;
		goto error;
	}
	if (readtables(M_SOF0, &isInitHuffman))
	{
		err = ERR_BAD_TABLES;
		goto error;
	}
	getword();
	i = getbyte();
	if (i != 8)
	{
		err = ERR_NOT_8BIT;
		goto error;
	}
	intheight = getword();
	intwidth = getword();

	if ((intheight & 7) || (intwidth & 7))
	{
		err = ERR_BAD_WIDTH_OR_HEIGHT;
		goto error;
	}
	info.nc = getbyte();
	if (info.nc > MAXCOMP)
	{
		err = ERR_TOO_MANY_COMPPS;
		goto error;
	}
	for (i = 0; i < info.nc; i++)
	{
		int h, v;
		comps[i].cid = getbyte();
		comps[i].hv = getbyte();
		v = comps[i].hv & 15;
		h = comps[i].hv >> 4;
		comps[i].tq = getbyte();
		if (h > 3 || v > 3)
		{
			err = ERR_ILLEGAL_HV;
			goto error;
		}
		if (comps[i].tq > 3)
		{
			err = ERR_QUANT_TABLE_SELECTOR;
			goto error;
		}
	}
	if (readtables(M_SOS, &isInitHuffman))
	{
		err = ERR_BAD_TABLES;
		goto error;
	}
	getword();
	info.ns = getbyte();
	if (!info.ns)
	{
		printf("info ns %d/n", info.ns);
		err = ERR_NOT_YCBCR_221111;
		goto error;
	}
	for (i = 0; i < info.ns; i++)
	{
		dscans[i].cid = getbyte();
		tdc = getbyte();
		tac = tdc & 15;
		tdc >>= 4;
		if (tdc > 1 || tac > 1)
		{
			err = ERR_QUANT_TABLE_SELECTOR;
			goto error;
		}
		for (j = 0; j < info.nc; j++)
			if (comps[j].cid == dscans[i].cid)
				break;
		if (j == info.nc)
		{
			err = ERR_UNKNOWN_CID_IN_SCAN;
			goto error;
		}
		dscans[i].hv = comps[j].hv;
		dscans[i].tq = comps[j].tq;
		dscans[i].hudc.dhuff = dec_huffdc + tdc;
		dscans[i].huac.dhuff = dec_huffac + tac;
	}

	i = getbyte();
	j = getbyte();
	m = getbyte();

	if (i != 0 || j != 63 || m != 0)
	{
		printf("hmm FW error,not seq DCT ??\n");
	}
	// printf("ext huffman table %d \n",isInitHuffman);
	if (!isInitHuffman)
	{
		if (huffman_init() < 0)
			return -ERR_BAD_TABLES;
	}
	/*
	 if (dscans[0].cid != 1 || dscans[1].cid != 2 || dscans[2].cid != 3) {
	 err = ERR_NOT_YCBCR_221111;
	 goto error;
	 }

	 if (dscans[1].hv != 0x11 || dscans[2].hv != 0x11) {
	 err = ERR_NOT_YCBCR_221111;
	 goto error;
	 }
	 */
	/* if internal width and external are not the same or heigth too
	 and pic not allocated realloc the good size and mark the change
	 need 1 macroblock line more ?? */
	if (intwidth != *width || intheight != *height || *pic == NULL)
	{
		*width = intwidth;
		*height = intheight;
		// BytesperPixel 2 yuyv , 3 rgb24
		*pic = (unsigned char *) realloc((unsigned char *) *pic,
				(size_t) intwidth * (intheight + 8) * 2);
	}

	switch (dscans[0].hv)
	{
	case 0x22: // 411
		mb = 6;
		mcusx = *width >> 4;
		mcusy = *height >> 4;
		bpp = 2;
		xpitch = 16 * bpp;
		pitch = *width * bpp; // YUYV out
		ypitch = 16 * pitch;
		convert = yuv420pto422;
		break;
	case 0x21: //422
		// printf("find 422 %dx%d\n",*width,*height);
		mb = 4;
		mcusx = *width >> 4;
		mcusy = *height >> 3;
		bpp = 2;
		xpitch = 16 * bpp;
		pitch = *width * bpp; // YUYV out
		ypitch = 8 * pitch;
		convert = yuv422pto422;
		break;
	case 0x11: //444
		mcusx = *width >> 3;
		mcusy = *height >> 3;
		bpp = 2;
		xpitch = 8 * bpp;
		pitch = *width * bpp; // YUYV out
		ypitch = 8 * pitch;
		if (info.ns == 1)
		{
			mb = 1;
			convert = yuv400pto422;
		}
		else
		{
			mb = 3;
			convert = yuv444pto422;
		}
		break;
	default:
		err = ERR_NOT_YCBCR_221111;
		goto error;
		break;
	}

	idctqtab(quant[dscans[0].tq], decdata->dquant[0]);
	idctqtab(quant[dscans[1].tq], decdata->dquant[1]);
	idctqtab(quant[dscans[2].tq], decdata->dquant[2]);
	setinput(&in, datap);
	dec_initscans();

	dscans[0].next = 2;
	dscans[1].next = 1;
	dscans[2].next = 0; /* 4xx encoding */
	for (my = 0, y = 0; my < mcusy; my++, y += ypitch)
	{
		for (mx = 0, x = 0; mx < mcusx; mx++, x += xpitch)
		{
			if (info.dri && !--info.nm)
				if (dec_checkmarker())
				{
					err = ERR_WRONG_MARKER;
					goto error;
				}
			switch (mb)
			{
			case 6:
			{
				decode_mcus(&in, decdata->dcts, mb, dscans, max);
				idct(decdata->dcts, decdata->out, decdata->dquant[0],
						IFIX(128.5), max[0]);
				idct(decdata->dcts + 64, decdata->out + 64, decdata->dquant[0],
						IFIX(128.5), max[1]);
				idct(decdata->dcts + 128, decdata->out + 128,
						decdata->dquant[0], IFIX(128.5), max[2]);
				idct(decdata->dcts + 192, decdata->out + 192,
						decdata->dquant[0], IFIX(128.5), max[3]);
				idct(decdata->dcts + 256, decdata->out + 256,
						decdata->dquant[1], IFIX(0.5), max[4]);
				idct(decdata->dcts + 320, decdata->out + 320,
						decdata->dquant[2], IFIX(0.5), max[5]);

			}
				break;
			case 4:
			{
				decode_mcus(&in, decdata->dcts, mb, dscans, max);
				idct(decdata->dcts, decdata->out, decdata->dquant[0],
						IFIX(128.5), max[0]);
				idct(decdata->dcts + 64, decdata->out + 64, decdata->dquant[0],
						IFIX(128.5), max[1]);
				idct(decdata->dcts + 128, decdata->out + 256,
						decdata->dquant[1], IFIX(0.5), max[4]);
				idct(decdata->dcts + 192, decdata->out + 320,
						decdata->dquant[2], IFIX(0.5), max[5]);

			}
				break;
			case 3:
				decode_mcus(&in, decdata->dcts, mb, dscans, max);
				idct(decdata->dcts, decdata->out, decdata->dquant[0],
						IFIX(128.5), max[0]);
				idct(decdata->dcts + 64, decdata->out + 256, decdata->dquant[1],
						IFIX(0.5), max[4]);
				idct(decdata->dcts + 128, decdata->out + 320,
						decdata->dquant[2], IFIX(0.5), max[5]);

				break;
			case 1:
				decode_mcus(&in, decdata->dcts, mb, dscans, max);
				idct(decdata->dcts, decdata->out, decdata->dquant[0],
						IFIX(128.5), max[0]);

				break;

			} // switch enc411
			convert(decdata->out, *pic + y + x, pitch);
		}
	}

	m = dec_readmarker(&in);
	if (m != M_EOI)
	{
		err = ERR_NO_EOI;
		goto error;
	}
	if (decdata)
		free(decdata);
	return 0;
	error: if (decdata)
		free(decdata);
	return err;
}

/****************************************************************/
/**************       huffman decoder             ***************/
/****************************************************************/
static int huffman_init(void)
{
	int tc, th, tt;
	const unsigned char *ptr = JPEGHuffmanTable;
	int i, j, l;
	l = JPG_HUFFMAN_TABLE_LENGTH;
	while (l > 0)
	{
		int hufflen[16], k;
		unsigned char huffvals[256];

		tc = *ptr++;
		th = tc & 15;
		tc >>= 4;
		tt = tc * 2 + th;
		if (tc > 1 || th > 1)
			return -ERR_BAD_TABLES;
		for (i = 0; i < 16; i++)
			hufflen[i] = *ptr++;
		l -= 1 + 16;
		k = 0;
		for (i = 0; i < 16; i++)
		{
			for (j = 0; j < hufflen[i]; j++)
				huffvals[k++] = *ptr++;
			l -= hufflen[i];
		}
		dec_makehuff(dhuff + tt, hufflen, huffvals);
	}
	return 0;
}

static int fillbits
__P((struct in *, int, unsigned int));

static int dec_rec2
__P((struct in *, struct dec_hufftbl *, int *, int, int));

static void setinput(struct in *in, unsigned char *p)
{
	in->p = p;
	in->left = 0;
	in->bits = 0;
	in->marker = 0;
}

static int fillbits(struct in *in, int le, unsigned int bi)
{
	int b, m;

	if (in->marker)
	{
		if (le <= 16)
			in->bits = bi << 16, le += 16;
		return le;
	}
	while (le <= 24)
	{
		b = *in->p++;
		if (b == 0xff && (m = *in->p++) != 0)
		{
			if (m == M_EOF)
			{
				if (in->func && (m = in->func(in->data)) == 0)
					continue;
			}
			in->marker = m;
			if (le <= 16)
				bi = bi << 16, le += 16;
			break;
		}
		bi = bi << 8 | b;
		le += 8;
	}
	in->bits = bi; /* tmp... 2 return values needed */
	return le;
}

static int dec_readmarker(struct in *in)
{
	int m;

	in->left = fillbits(in, in->left, in->bits);
	if ((m = in->marker) == 0)
		return 0;
	in->left = 0;
	in->marker = 0;
	return m;
}

#define LEBI_DCL	int le, bi
#define LEBI_GET(in)	(le = in->left, bi = in->bits)
#define LEBI_PUT(in)	(in->left = le, in->bits = bi)

#define GETBITS(in, n) (					\
  (le < (n) ? le = fillbits(in, le, bi), bi = in->bits : 0),	\
  (le -= (n)),							\
  bi >> le & ((1 << (n)) - 1)					\
)

#define UNGETBITS(in, n) (	\
  le += (n)			\
)

static int dec_rec2(struct in *in, struct dec_hufftbl *hu, int *runp, int c,
		int i)
{
	LEBI_DCL;

	LEBI_GET(in);
	if (i)
	{
		UNGETBITS(in, i & 127);
		*runp = i >> 8 & 15;
		i >>= 16;
	}
	else
	{
		for (i = DECBITS; (c = ((c << 1) | GETBITS(in, 1))) >= (hu->maxcode[i]);
				i++)
			;
		if (i >= 16)
		{
			in->marker = M_BADHUFF;
			return 0;
		}
		i = hu->vals[hu->valptr[i] + c - hu->maxcode[i - 1] * 2];
		*runp = i >> 4;
		i &= 15;
	}
	if (i == 0)
	{ /* sigh, 0xf0 is 11 bit */
		LEBI_PUT(in);
		return 0;
	}
	/* receive part */
	c = GETBITS(in, i);
	if (c < (1 << (i - 1)))
		c += (-1 << i) + 1;
	LEBI_PUT(in);
	return c;
}

#define DEC_REC(in, hu, r, i)	 (	\
  r = GETBITS(in, DECBITS),		\
  i = hu->llvals[r],			\
  i & 128 ?				\
    (					\
      UNGETBITS(in, i & 127),		\
      r = i >> 8 & 15,			\
      i >> 16				\
    )					\
  :					\
    (					\
      LEBI_PUT(in),			\
      i = dec_rec2(in, hu, &r, r, i),	\
      LEBI_GET(in),			\
      i					\
    )					\
)

static void decode_mcus(struct in *in, int *dct, int n, struct scan *sc,
		int *maxp)
{
	struct dec_hufftbl *hu;
	int i, r, t;
	LEBI_DCL;

	memset(dct, 0, n * 64 * sizeof(*dct));
	LEBI_GET(in);
	while (n-- > 0)
	{
		hu = sc->hudc.dhuff;
		*dct++ = (sc->dc += DEC_REC(in, hu, r, t));

		hu = sc->huac.dhuff;
		i = 63;
		while (i > 0)
		{
			t = DEC_REC(in, hu, r, t);
			if (t == 0 && r == 0)
			{
				dct += i;
				break;
			}
			dct += r;
			*dct++ = t;
			i -= r + 1;
		}
		*maxp++ = 64 - i;
		if (n == sc->next)
			sc++;
	}
	LEBI_PUT(in);
}

static void dec_makehuff(struct dec_hufftbl *hu, int *hufflen,
		unsigned char *huffvals)
{
	int code, k, i, j, d, x, c, v;
	for (i = 0; i < (1 << DECBITS); i++)
		hu->llvals[i] = 0;

	/*
	 * llvals layout:
	 *
	 * value v already known, run r, backup u bits:
	 *  vvvvvvvvvvvvvvvv 0000 rrrr 1 uuuuuuu
	 * value unknown, size b bits, run r, backup u bits:
	 *  000000000000bbbb 0000 rrrr 0 uuuuuuu
	 * value and size unknown:
	 *  0000000000000000 0000 0000 0 0000000
	 */
	code = 0;
	k = 0;
	for (i = 0; i < 16; i++, code <<= 1)
	{ /* sizes */
		hu->valptr[i] = k;
		for (j = 0; j < hufflen[i]; j++)
		{
			hu->vals[k] = *huffvals++;
			if (i < DECBITS)
			{
				c = code << (DECBITS - 1 - i);
				v = hu->vals[k] & 0x0f; /* size */
				for (d = 1 << (DECBITS - 1 - i); --d >= 0;)
				{
					if (v + i < DECBITS)
					{ /* both fit in table */
						x = d >> (DECBITS - 1 - v - i);
						if (v && x < (1 << (v - 1)))
							x += (-1 << v) + 1;
						x = x << 16 | (hu->vals[k] & 0xf0) << 4
								| (DECBITS - (i + 1 + v)) | 128;
					}
					else
						x = v << 16 | (hu->vals[k] & 0xf0) << 4
								| (DECBITS - (i + 1));
					hu->llvals[c | d] = x;
				}
			}
			code++;
			k++;
		}
		hu->maxcode[i] = code;
	}
	hu->maxcode[16] = 0x20000; /* always terminate decode */
}

/****************************************************************/
/**************             idct                  ***************/
/****************************************************************/

#define IMULT(a, b) (((a) * (b)) >> ISHIFT)
#define ITOINT(a) ((a) >> ISHIFT)

#define S22 ((PREC)IFIX(2 * 0.382683432))
#define C22 ((PREC)IFIX(2 * 0.923879532))
#define IC4 ((PREC)IFIX(1 / 0.707106781))

static unsigned char zig2[64] =
{ 0, 2, 3, 9, 10, 20, 21, 35, 14, 16, 25, 31, 39, 46, 50, 57, 5, 7, 12, 18, 23,
		33, 37, 48, 27, 29, 41, 44, 52, 55, 59, 62, 15, 26, 30, 40, 45, 51, 56,
		58, 1, 4, 8, 11, 19, 22, 34, 36, 28, 42, 43, 53, 54, 60, 61, 63, 6, 13,
		17, 24, 32, 38, 47, 49 };

inline static void idct(int *in, int *out, int *quant, long off, int max)
{
	long t0, t1, t2, t3, t4, t5, t6, t7;	// t ;
	long tmp0, tmp1, tmp2, tmp3, tmp4, tmp5, tmp6;
	long tmp[64], *tmpp;
	int i, j, te;
	unsigned char *zig2p;

	t0 = off;
	if (max == 1)
	{
		t0 += in[0] * quant[0];
		for (i = 0; i < 64; i++)
			out[i] = ITOINT(t0);
		return;
	}
	zig2p = zig2;
	tmpp = tmp;
	for (i = 0; i < 8; i++)
	{
		j = *zig2p++;
		t0 += in[j] * (long) quant[j];
		j = *zig2p++;
		t5 = in[j] * (long) quant[j];
		j = *zig2p++;
		t2 = in[j] * (long) quant[j];
		j = *zig2p++;
		t7 = in[j] * (long) quant[j];
		j = *zig2p++;
		t1 = in[j] * (long) quant[j];
		j = *zig2p++;
		t4 = in[j] * (long) quant[j];
		j = *zig2p++;
		t3 = in[j] * (long) quant[j];
		j = *zig2p++;
		t6 = in[j] * (long) quant[j];

		if ((t1 | t2 | t3 | t4 | t5 | t6 | t7) == 0)
		{

			tmpp[0 * 8] = t0;
			tmpp[1 * 8] = t0;
			tmpp[2 * 8] = t0;
			tmpp[3 * 8] = t0;
			tmpp[4 * 8] = t0;
			tmpp[5 * 8] = t0;
			tmpp[6 * 8] = t0;
			tmpp[7 * 8] = t0;

			tmpp++;
			t0 = 0;
			continue;
		}
		//IDCT;
		tmp0 = t0 + t1;
		t1 = t0 - t1;
		tmp2 = t2 - t3;
		t3 = t2 + t3;
		tmp2 = IMULT(tmp2, IC4) - t3;
		tmp3 = tmp0 + t3;
		t3 = tmp0 - t3;
		tmp1 = t1 + tmp2;
		tmp2 = t1 - tmp2;
		tmp4 = t4 - t7;
		t7 = t4 + t7;
		tmp5 = t5 + t6;
		t6 = t5 - t6;
		tmp6 = tmp5 - t7;
		t7 = tmp5 + t7;
		tmp5 = IMULT(tmp6, IC4);
		tmp6 = IMULT((tmp4 + t6), S22);
		tmp4 = IMULT(tmp4, (C22 - S22)) + tmp6;
		t6 = IMULT(t6, (C22 + S22)) - tmp6;
		t6 = t6 - t7;
		t5 = tmp5 - t6;
		t4 = tmp4 - t5;

		tmpp[0 * 8] = tmp3 + t7;	//t0;
		tmpp[1 * 8] = tmp1 + t6;	//t1;
		tmpp[2 * 8] = tmp2 + t5;	//t2;
		tmpp[3 * 8] = t3 + t4;	//t3;
		tmpp[4 * 8] = t3 - t4;	//t4;
		tmpp[5 * 8] = tmp2 - t5;	//t5;
		tmpp[6 * 8] = tmp1 - t6;	//t6;
		tmpp[7 * 8] = tmp3 - t7;	//t7;
		tmpp++;
		t0 = 0;
	}
	for (i = 0, j = 0; i < 8; i++)
	{
		t0 = tmp[j + 0];
		t1 = tmp[j + 1];
		t2 = tmp[j + 2];
		t3 = tmp[j + 3];
		t4 = tmp[j + 4];
		t5 = tmp[j + 5];
		t6 = tmp[j + 6];
		t7 = tmp[j + 7];
		if ((t1 | t2 | t3 | t4 | t5 | t6 | t7) == 0)
		{
			te = ITOINT(t0);
			out[j + 0] = te;
			out[j + 1] = te;
			out[j + 2] = te;
			out[j + 3] = te;
			out[j + 4] = te;
			out[j + 5] = te;
			out[j + 6] = te;
			out[j + 7] = te;
			j += 8;
			continue;
		}
		//IDCT;
		tmp0 = t0 + t1;
		t1 = t0 - t1;
		tmp2 = t2 - t3;
		t3 = t2 + t3;
		tmp2 = IMULT(tmp2, IC4) - t3;
		tmp3 = tmp0 + t3;
		t3 = tmp0 - t3;
		tmp1 = t1 + tmp2;
		tmp2 = t1 - tmp2;
		tmp4 = t4 - t7;
		t7 = t4 + t7;
		tmp5 = t5 + t6;
		t6 = t5 - t6;
		tmp6 = tmp5 - t7;
		t7 = tmp5 + t7;
		tmp5 = IMULT(tmp6, IC4);
		tmp6 = IMULT((tmp4 + t6), S22);
		tmp4 = IMULT(tmp4, (C22 - S22)) + tmp6;
		t6 = IMULT(t6, (C22 + S22)) - tmp6;
		t6 = t6 - t7;
		t5 = tmp5 - t6;
		t4 = tmp4 - t5;

		out[j + 0] = ITOINT(tmp3 + t7);
		out[j + 1] = ITOINT(tmp1 + t6);
		out[j + 2] = ITOINT(tmp2 + t5);
		out[j + 3] = ITOINT(t3 + t4);
		out[j + 4] = ITOINT(t3 - t4);
		out[j + 5] = ITOINT(tmp2 - t5);
		out[j + 6] = ITOINT(tmp1 - t6);
		out[j + 7] = ITOINT(tmp3 - t7);
		j += 8;
	}

}

static unsigned char zig[64] =
{ 0, 1, 5, 6, 14, 15, 27, 28, 2, 4, 7, 13, 16, 26, 29, 42, 3, 8, 12, 17, 25, 30,
		41, 43, 9, 11, 18, 24, 31, 40, 44, 53, 10, 19, 23, 32, 39, 45, 52, 54,
		20, 22, 33, 38, 46, 51, 55, 60, 21, 34, 37, 47, 50, 56, 59, 61, 35, 36,
		48, 49, 57, 58, 62, 63 };

static PREC aaidct[8] =
{ IFIX(0.3535533906), IFIX(0.4903926402), IFIX(0.4619397663), IFIX(
		0.4157348062), IFIX(0.3535533906), IFIX(0.2777851165), IFIX(
		0.1913417162), IFIX(0.0975451610) };

static void idctqtab(unsigned char *qin,
PREC *qout)
{
	int i, j;

	for (i = 0; i < 8; i++)
		for (j = 0; j < 8; j++)
			qout[zig[i * 8 + j]] = qin[zig[i * 8 + j]]
					* IMULT(aaidct[i], aaidct[j]);
}

#define  FOUR_TWO_TWO 2		//Y00 Cb Y01 Cr

/* translate YUV422Packed to rgb24 */

unsigned int Pyuv422torgb24(unsigned char * input_ptr,
		unsigned char * output_ptr, unsigned int image_width,
		unsigned int image_height)
{
	unsigned int i, size;
	unsigned char Y, Y1, U, V;
	unsigned char *buff = input_ptr;
	unsigned char *output_pt = output_ptr;
	size = image_width * image_height / 2;
	for (i = size; i > 0; i--)
	{
		/* bgr instead rgb ?? */
		Y = buff[0];
		U = buff[1];
		Y1 = buff[2];
		V = buff[3];
		buff += 4;
		*output_pt++ = R_FROMYV(Y, V);
		*output_pt++ = G_FROMYUV(Y, U, V); //b
		*output_pt++ = B_FROMYU(Y, U); //v

		*output_pt++ = R_FROMYV(Y1, V);
		*output_pt++ = G_FROMYUV(Y1, U, V); //b
		*output_pt++ = B_FROMYU(Y1, U); //v
	}

	return FOUR_TWO_TWO;
}

static void yuv420pto422(int * out, unsigned char *pic, int width)
{
	int j, k;
	unsigned char *pic0, *pic1;
	int *outy, *outu, *outv;
	int outy1 = 0;
	int outy2 = 8;

	pic0 = pic;
	pic1 = pic + width;
	outy = out;
	outu = out + 64 * 4;
	outv = out + 64 * 5;
	for (j = 0; j < 8; j++)
	{
		for (k = 0; k < 8; k++)
		{
			if (k == 4)
			{
				outy1 += 56;
				outy2 += 56;
			}
			*pic0++ = CLIP(outy[outy1]);
			*pic0++ = CLIP(128 + *outu);
			*pic0++ = CLIP(outy[outy1 + 1]);
			*pic0++ = CLIP(128 + *outv);
			*pic1++ = CLIP(outy[outy2]);
			*pic1++ = CLIP(128 + *outu);
			*pic1++ = CLIP(outy[outy2 + 1]);
			*pic1++ = CLIP(128 + *outv);
			outy1 += 2;
			outy2 += 2;
			outu++;
			outv++;
		}
		if (j == 3)
		{
			outy = out + 128;
		}
		else
		{
			outy += 16;
		}
		outy1 = 0;
		outy2 = 8;
		pic0 += 2 * (width - 16);
		pic1 += 2 * (width - 16);

	}

}
static void yuv422pto422(int * out, unsigned char *pic, int width)
{
	int j, k;
	unsigned char *pic0, *pic1;
	int *outy, *outu, *outv;
	int outy1 = 0;
	int outy2 = 8;
	int outu1 = 0;
	int outv1 = 0;

	pic0 = pic;
	pic1 = pic + width;
	outy = out;
	outu = out + 64 * 4;
	outv = out + 64 * 5;
	for (j = 0; j < 4; j++)
	{
		for (k = 0; k < 8; k++)
		{
			if (k == 4)
			{
				outy1 += 56;
				outy2 += 56;
			}
			*pic0++ = CLIP(outy[outy1]);
			*pic0++ = CLIP(128 + outu[outu1]);
			*pic0++ = CLIP(outy[outy1 + 1]);
			*pic0++ = CLIP(128 + outv[outv1]);
			*pic1++ = CLIP(outy[outy2]);
			*pic1++ = CLIP(128 + outu[outu1 + 8]);
			*pic1++ = CLIP(outy[outy2 + 1]);
			*pic1++ = CLIP(128 + outv[outv1 + 8]);
			outv1 += 1;
			outu1 += 1;
			outy1 += 2;
			outy2 += 2;

		}

		outy += 16;
		outu += 8;
		outv += 8;
		outv1 = 0;
		outu1 = 0;
		outy1 = 0;
		outy2 = 8;
		pic0 += 2 * (width - 16);
		pic1 += 2 * (width - 16);

	}

}
static void yuv444pto422(int * out, unsigned char *pic, int width)
{
	int j, k;
	unsigned char *pic0, *pic1;
	int *outy, *outu, *outv;
	int outy1 = 0;
	int outy2 = 8;
	int outu1 = 0;
	int outv1 = 0;

	pic0 = pic;
	pic1 = pic + width;
	outy = out;
	outu = out + 64 * 4; // Ooops where did i invert ??
	outv = out + 64 * 5;
	for (j = 0; j < 4; j++)
	{
		for (k = 0; k < 4; k++)
		{

			*pic0++ = CLIP(outy[outy1]);
			*pic0++ = CLIP(128 + outu[outu1]);
			*pic0++ = CLIP(outy[outy1 + 1]);
			*pic0++ = CLIP(128 + outv[outv1]);
			*pic1++ = CLIP(outy[outy2]);
			*pic1++ = CLIP(128 + outu[outu1 + 8]);
			*pic1++ = CLIP(outy[outy2 + 1]);
			*pic1++ = CLIP(128 + outv[outv1 + 8]);
			outv1 += 2;
			outu1 += 2;
			outy1 += 2;
			outy2 += 2;
		}
		outy += 16;
		outu += 16;
		outv += 16;
		outv1 = 0;
		outu1 = 0;
		outy1 = 0;
		outy2 = 8;
		pic0 += 2 * (width - 8);
		pic1 += 2 * (width - 8);
	}

}
static void yuv400pto422(int * out, unsigned char *pic, int width)
{
	int j, k;
	unsigned char *pic0, *pic1;
	int *outy;
	int outy1 = 0;
	int outy2 = 8;
	pic0 = pic;
	pic1 = pic + width;
	outy = out;

	for (j = 0; j < 4; j++)
	{
		for (k = 0; k < 4; k++)
		{
			*pic0++ = CLIP(outy[outy1]);
			*pic0++ = 128;
			*pic0++ = CLIP(outy[outy1 + 1]);
			*pic0++ = 128;
			*pic1++ = CLIP(outy[outy2]);
			*pic1++ = 128;
			*pic1++ = CLIP(outy[outy2 + 1]);
			*pic1++ = 128;
			outy1 += 2;
			outy2 += 2;
		}
		outy += 16;
		outy1 = 0;
		outy2 = 8;
		pic0 += 2 * (width - 8);
		pic1 += 2 * (width - 8);
	}

}

int is_huffman(unsigned char *buf)
{
	unsigned char *ptbuf;
	int i = 0;
	ptbuf = buf;
	while (((ptbuf[0] << 8) | ptbuf[1]) != 0xffda)
	{
		if (i++ > 2048)
			return 0;
		if (((ptbuf[0] << 8) | ptbuf[1]) == 0xffc4)
			return 1;
		ptbuf++;
	}
	return 0;
}
static void getPictureName(char *Picture, int fmt)
{
	char temp[80];
	char *myext[] =
	{ "pnm", "jpg" };
//  int i;
	time_t curdate;
	struct tm *tdate;
	memset(temp, '\0', sizeof(temp));
	time(&curdate);
	tdate = localtime(&curdate);
	snprintf(temp, 26, "P-%02d:%02d:%04d-%02d:%02d:%02d.%s", tdate->tm_mon + 1,
			tdate->tm_mday, tdate->tm_year + 1900, tdate->tm_hour,
			tdate->tm_min, tdate->tm_sec, myext[fmt]);

	memcpy(Picture, temp, strlen(temp));
}

int get_picture(unsigned char *buf, int size)
{
	FILE *file;
	unsigned char *ptdeb, *ptcur = buf;
	int sizein;
	char *name = NULL;
	name = (char *)calloc(80, 1);
	getPictureName(name, 1);
	file = fopen(name, "wb");
	if (file != NULL)
	{
		if (!is_huffman(buf))
		{
			ptdeb = ptcur = buf;
			while (((ptcur[0] << 8) | ptcur[1]) != 0xffc0)
				ptcur++;
			sizein = ptcur - ptdeb;
			fwrite(buf, sizein, 1, file);
			fwrite(dht_data, DHT_SIZE, 1, file);
			fwrite(ptcur, size - sizein, 1, file);
		}
		else
		{
			fwrite(ptcur, size, 1, file); /* ptcur was uninit -wsr */
		}
		fclose(file);
	}
	if (name)
		free(name);
	return 0;
}

int get_pictureYV2(unsigned char *buf, int width, int height)
{
	FILE *foutpict;
	unsigned char *picture = NULL;
	char *name = NULL;
	name = (char *)calloc(80, 1);
	getPictureName(name, 0);
	picture = (unsigned char *) malloc(width * height * 3 * sizeof(char));
	if (picture)
	{
		Pyuv422torgb24(buf, picture, width, height);
	}
	else
	{
		printf(" no room to take a picture \n");
		return 0;
	}
	if (name)
	{
		foutpict = fopen(name, "wb");
		fprintf(foutpict, "P6\n%d %d\n255\n", width, height);
		fwrite(picture, sizeof(char), width * height * 3, foutpict);
		fclose(foutpict);
		free(name);
	}
	free(picture);
	picture = NULL;
	return 0;
}


#ifdef __cplusplus
}
#endif


